# CUSTOM FUNCTIONS BY GILLES
import math
from constants import *

def give_grid_number(x, y):
    grid_x = math.floor((x / XDIM) * GRID_WIDTH)
    grid_y = math.floor((y / YDIM) * GRID_HEIGHT)
    cell_n = grid_x + GRID_HEIGHT * grid_y
    return int(cell_n)
    
def give_grid_neighbour(grid_cell, offX, offY): # give one (almost) neighbour number: left/right/upper or down 
    grid_x = (grid_cell%GRID_WIDTH) + offX
    grid_y = (grid_cell//GRID_HEIGHT) + offY
    grid_neigh = grid_x + GRID_HEIGHT * grid_y

    if grid_x < 0 or grid_y < 0 or grid_x >= GRID_WIDTH or grid_y >= GRID_HEIGHT:
        grid_neigh = -1 # outside field
    
    #print '%i offset %i %i -> %i' % (grid_cell, offX, offY, grid_neigh)
    return grid_neigh
        
def give_crumbs_in_grid(grid_numbers, crumbs, grid_n):
    crumbs_in_grid = []
    
    for crumb in crumbs:
        if(grid_numbers[(crumb.x, crumb.y)] == grid_n):
            crumbs_in_grid.append(crumb)
                
    return crumbs_in_grid
    
    #return [c for c in crumbs if give_grid_number(c.x, c.y) == grid_n]    
            
def give_closest_crumb(crumbs, my_x, my_y):
    closest_distance = XDIM * YDIM
    closest_crumb = (my_x, my_y)
    
    for c in crumbs:
        current_distance = get_distance((my_x, my_y), (c.x, c.y))
        if(current_distance < closest_distance):
            closest_crumb = (c.x, c.y)
            closest_distance = current_distance    
    return closest_crumb

def give_central_crumb(crumbs, grid_cell):
    (x,y) = give_grid_center(grid_cell)
    # print 'grid %d centre: %.1f,%.1f'%(grid_cell, x,y)
    return give_closest_crumb(crumbs, x, y)
    
def give_grid_center(grid_cell):
    grid_x = grid_cell%GRID_WIDTH
    grid_y = grid_cell//GRID_WIDTH  ## HEIGHT?
    x = ((grid_x+0.5)/GRID_WIDTH) * XDIM
    y = ((grid_y+0.5)/GRID_HEIGHT) * YDIM
    return (x,y)    

def give_nsew_options(grid):
    options = [grid]
    if(grid >= GRID_WIDTH):
        options += [grid - GRID_WIDTH]
    
    if(grid < N_GRID - GRID_WIDTH):
        options += [grid + GRID_WIDTH]
    
    if(grid % GRID_WIDTH != GRID_WIDTH - 1):
        options += [grid + 1]

    if(grid % GRID_WIDTH > 0):
        options += [grid - 1]
    
    return options
        
def get_grid_from_state(state):
    
    for i in range(1, len(state), 3):
        if state[i] == 1:
            return i//3
    raise Exception("Agent not in a state")
    

def give_relative_grid(my_grid, his_grid):
    if my_grid == his_grid:
        return 8
    else:
    
        x_diff = his_grid % GRID_WIDTH - my_grid % GRID_WIDTH
        y_diff = math.floor(his_grid / GRID_WIDTH) - math.floor(my_grid / GRID_WIDTH)
        
        if(x_diff == -1 and y_diff == -1):
            return 0
        elif(x_diff == 0 and y_diff == -1):
            return 1
        elif(x_diff == 1 and y_diff == -1):
            return 2
        elif(x_diff == -1 and y_diff == 0):
            return 3
        elif(x_diff == 1 and y_diff == 0):
            return 4
        elif(x_diff == -1 and y_diff == 1):
            return 5
        elif(x_diff == 0 and y_diff == 1):
            return 6
        elif(x_diff == 1 and y_diff == 1):        
            return 7
        else:
            return None

def give_absolute_grid(my_grid, relative_grid):
    
    abs_grid = -1
    
    if relative_grid == 0:
        abs_grid = my_grid - GRID_WIDTH - 1        
    if relative_grid == 1:
        abs_grid = my_grid - GRID_WIDTH
        #print "YO"
    if relative_grid == 2:
        abs_grid = my_grid - GRID_WIDTH + 1
    if relative_grid == 3:
        abs_grid = my_grid - 1
    if relative_grid == 4:
        abs_grid = my_grid + 1
    if relative_grid == 5:
        abs_grid = my_grid + GRID_WIDTH - 1    
    if relative_grid == 6:
        abs_grid = my_grid + GRID_WIDTH    
    if relative_grid == 7:
        abs_grid = my_grid + GRID_WIDTH  + 1    
        
    if abs_grid > -1 and abs_grid < N_GRID:
        return abs_grid
    else:
        return None


                
def discretize_n_crumbs(n_crumbs):
    if n_crumbs == 0:
        return 0
    
    for i in range(0, len(DISCRETE_CRUMB_LEVELS)):
        if n_crumbs < DISCRETE_CRUMB_LEVELS[i]:
            return i + 1
            
    return len(DISCRETE_CRUMB_LEVELS) + 1
           
def give_neighbor_grids(grid_n):
    neighbor_grids = []
    # NW
    if(grid_n > GRID_WIDTH and grid_n % GRID_WIDTH > 0):
        neighbor_grids.append(grid_n - GRID_WIDTH - 1)
    # N
    if(grid_n > GRID_WIDTH):
        neighbor_grids.append(grid_n - GRID_WIDTH)
       
    # NE
    if(grid_n > GRID_WIDTH and grid_n % GRID_WIDTH < (GRID_WIDTH-1)):
        neighbor_grids.append(grid_n - GRID_WIDTH + 1)

    # W
    if(grid_n % GRID_WIDTH > 0):
        neighbor_grids.append(grid_n-1)
        
    # E
    if(grid_n % GRID_WIDTH < (GRID_WIDTH-1)):
        neighbor_grids.append(grid_n+1)
        
    # SW
    if(grid_n < (GRID_HEIGHT-1) * GRID_WIDTH and grid_n % GRID_WIDTH > 0):
        neighbor_grids.append(grid_n + GRID_WIDTH - 1)
    
    # S
    if(grid_n < (GRID_HEIGHT-1) * GRID_WIDTH):
        neighbor_grids.append(grid_n + GRID_WIDTH)
    
    # SE
    if(grid_n < (GRID_HEIGHT-1) * GRID_WIDTH and grid_n % GRID_WIDTH < (GRID_WIDTH-1) ):
        neighbor_grids.append(grid_n + GRID_WIDTH + 1)
    
    return neighbor_grids

def give_new_neighbor(grid_n):
    if(grid_n > GRID_WIDTH and grid_n % GRID_WIDTH > 0):
        return grid_n - GRID_WIDTH - 1

def give_n_neighbor(grid_n):
    if(grid_n > GRID_WIDTH):
        return grid_n - GRID_WIDTH
        
def give_ne_neighbor(grid_n):
    if(grid_n > GRID_WIDTH and grid_n % GRID_WIDTH < (GRID_WIDTH-1)):
        return grid_n - GRID_WIDTH + 1
    else:
        return None
        
def get_distance((a_x, a_y),(b_x, b_y)):
    """
    Returns distance
    """
    return math.sqrt( (a_x-b_x)**2 + (a_y-b_y)**2 )
